// PID

#ifndef H_PID
#define H_PID

#include "Arduino.h"

class Pid
{
private:
  float oldval;
  float alphad; // smoothing newval, to create a front to calculate derivative 
  float der; // TODO Remove from here not public
  float integral; // TODO Remove from here not public


  unsigned long last,now;


public:
  float p,i,d;

  void setup(float _p,float _i,float _d)
  {
    p=_p;
    i=_i;
    d=_d;
    alphad=0.75; // empirical used for derivative
    now=micros();
  }
  
  void save()
  {
    para.writeFloat(p);
    para.writeFloat(i);
    para.writeFloat(d);
  }
  
  void load()
  {
    p=para.readFloat();
    i=para.readFloat();
    d=para.readFloat();
  }

  float update(float setpoint, float newval) // command is returned dt is in seconds
  {
    last=now;
    now=micros();
    float dt=(now-last)/1e6;

    float error=(setpoint-newval);

    integral+=error*dt;
    if (integral > 25.0) integral=25.0;
    if (integral < -25.0) integral=-25.0;

    oldval=oldval*alphad+error*(1-alphad);
    der=(error-oldval)/dt/10.0; // positive if error is increasing so you must increase also the command

    return error*p+integral*i+der*d;
  }

  void reset()
  {
    integral=0;
  }
} 
pRoll,pPitch,pYaw;

#endif





